#pragma once

#include "uwb_dt/uwb_dt_publisher.hpp"
#include "nlink_parser/LinktrackNodeframe0.h"
#include "ros/ros.h"
#include <vector>

class uwb_dt_handle
{
public:
    uwb_dt_handle() = delete;
    uwb_dt_handle(ros::NodeHandle& nh);
    ~uwb_dt_handle() = default;

private:
    std::map<uint8_t, std::map<uint8_t, std::string>> msg_buffer_;
    ros::Subscriber data_raw_sub_;
    std::vector<ros::Publisher> pubs_;

    void DataBridge(const std::string& str_msg);
    void DataRawCallback(const nlink_parser::LinktrackNodeframe0::ConstPtr& msg);

public:

    constexpr static uint8_t FRAME_HEAD = uwb_dt_publisher::FRAME_HEAD;
    constexpr static uint8_t FRAME_PAUSE = uwb_dt_publisher::FRAME_PAUSE;
    constexpr static uint8_t FRAME_RESUME = uwb_dt_publisher::FRAME_RESUME;
    constexpr static uint8_t FRAME_TAIL = uwb_dt_publisher::FRAME_TAIL;
    const std::map<std::pair<uint8_t, uint8_t>, uwb_dt_publisher::frame_type_t> HEAD_MAP = {
        {{FRAME_HEAD, FRAME_HEAD}, uwb_dt_publisher::frame_type_t::SINGLE},
        {{FRAME_HEAD, FRAME_HEAD}, uwb_dt_publisher::frame_type_t::HEAD},
        {{FRAME_HEAD, FRAME_RESUME}, uwb_dt_publisher::frame_type_t::MID},
        {{FRAME_HEAD, FRAME_RESUME}, uwb_dt_publisher::frame_type_t::TAIL},
    };
    const std::map<std::pair<uint8_t, uint8_t>, uwb_dt_publisher::frame_type_t> TAIL_MAP = {
        {{FRAME_TAIL, FRAME_TAIL}, uwb_dt_publisher::frame_type_t::SINGLE},
        {{FRAME_PAUSE, FRAME_TAIL}, uwb_dt_publisher::frame_type_t::HEAD},
        {{FRAME_PAUSE, FRAME_TAIL}, uwb_dt_publisher::frame_type_t::MID},
        {{FRAME_TAIL, FRAME_TAIL}, uwb_dt_publisher::frame_type_t::TAIL},
    };
};